// Copyright 2024 Polar Bear Team
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef DRONE_SERIAL_INTERFACE__DRONE_SERIAL_INTERFACE_NODE_HPP_
#define DRONE_SERIAL_INTERFACE__DRONE_SERIAL_INTERFACE_NODE_HPP_

#include <memory>
#include <string>
#include <chrono>
#include <thread>

#include <rclcpp/rclcpp.hpp>
#include <rclcpp_lifecycle/lifecycle_node.hpp>
#include <geometry_msgs/msg/twist.hpp>
#include <std_msgs/msg/string.hpp>
#include <diagnostic_msgs/msg/diagnostic_array.hpp>
#include <diagnostic_updater/diagnostic_updater.hpp>
#include <serial/serial.h>

#include "drone_serial_interface/protocol_handler.hpp"

namespace drone_serial_interface
{

/**
 * @brief 无人机串口通信接口节点
 * 
 * 该节点负责：
 * - 订阅来自导航系统的3D速度命令 (/cmd_vel)
 * - 将速度命令转换为无人机协议格式
 * - 通过串口发送命令给无人机飞控
 * - 监控通信状态和错误处理
 * - 提供诊断信息和状态反馈
 */
class DroneSerialInterfaceNode : public rclcpp_lifecycle::LifecycleNode
{
public:
  explicit DroneSerialInterfaceNode(const rclcpp::NodeOptions & options = rclcpp::NodeOptions());
  virtual ~DroneSerialInterfaceNode();

protected:
  // Lifecycle callbacks
  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
  on_configure(const rclcpp_lifecycle::State & state) override;
  
  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
  on_activate(const rclcpp_lifecycle::State & state) override;
  
  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
  on_deactivate(const rclcpp_lifecycle::State & state) override;
  
  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
  on_cleanup(const rclcpp_lifecycle::State & state) override;
  
  rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn
  on_shutdown(const rclcpp_lifecycle::State & state) override;

private:
  // ROS2 subscribers and publishers
  rclcpp::Subscription<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_sub_;
  rclcpp::Publisher<std_msgs::msg::String>::SharedPtr serial_status_pub_;
  rclcpp::Publisher<geometry_msgs::msg::Twist>::SharedPtr cmd_vel_feedback_pub_;
  
  // Serial communication
  std::unique_ptr<serial::Serial> serial_port_;
  std::string serial_device_;
  uint32_t serial_baudrate_;
  serial::Timeout serial_timeout_;
  
  // Communication parameters
  std::string protocol_type_;          // 协议类型 (custom, mavlink, etc.)
  double cmd_timeout_;                 // 命令超时时间
  double max_linear_velocity_;         // 最大线速度限制
  double max_angular_velocity_;        // 最大角速度限制
  bool enable_safety_checks_;          // 启用安全检查
  
  // Protocol handler
  std::unique_ptr<ProtocolHandler> protocol_handler_;
  
  // State management
  bool serial_connected_;
  std::chrono::steady_clock::time_point last_cmd_time_;
  geometry_msgs::msg::Twist last_cmd_vel_;
  
  // Diagnostics
  std::unique_ptr<diagnostic_updater::Updater> diagnostic_updater_;
  
  // Timers
  rclcpp::TimerBase::SharedPtr heartbeat_timer_;
  rclcpp::TimerBase::SharedPtr status_timer_;
  
  // Threading
  std::thread serial_thread_;
  std::atomic<bool> thread_running_;
  std::mutex cmd_vel_mutex_;
  
  // Parameter callback handle
  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr param_callback_handle_;
  
  // Callback functions
  void cmdVelCallback(const geometry_msgs::msg::Twist::SharedPtr msg);
  void heartbeatTimerCallback();
  void statusTimerCallback();
  
  // Serial communication functions
  bool initializeSerial();
  void closeSerial();
  bool sendVelocityCommand(const geometry_msgs::msg::Twist & cmd_vel);
  std::string formatVelocityCommand(const geometry_msgs::msg::Twist & cmd_vel);
  bool validateVelocityCommand(const geometry_msgs::msg::Twist & cmd_vel);
  
  // Safety and monitoring functions
  void emergencyStop();
  bool isCommandTimeout();
  void updateDiagnostics();
  
  // Diagnostic callbacks
  void diagnosticConnectionStatus(diagnostic_updater::DiagnosticStatusWrapper & stat);
  void diagnosticVelocityStatus(diagnostic_updater::DiagnosticStatusWrapper & stat);
  void diagnosticProtocolStatus(diagnostic_updater::DiagnosticStatusWrapper & stat);
  
  // Parameter callbacks
  void declareParameters();
  void getParameters();
  rcl_interfaces::msg::SetParametersResult parametersCallback(
    const std::vector<rclcpp::Parameter> & parameters);
  
  // Utility functions
  std::string getTimestamp();
  void logSerialError(const std::string & error_msg);
};

} // namespace drone_serial_interface

#endif // DRONE_SERIAL_INTERFACE__DRONE_SERIAL_INTERFACE_NODE_HPP_